#include  "KalmanFilter.h"
#include "opencv2\highgui\highgui.hpp";
#include <iostream>


using namespace std;
struct mouse_info_struct1 { int x,y; };
struct mouse_info_struct1 mouse_info = {-1,-1}, last_mouse;
void on_mouse1(int event, int x, int y, int flags, void* param) {
	{
		last_mouse = mouse_info;
		mouse_info.x = x;
		mouse_info.y = y;
	}
}




int run();

//int main()
//{
//	run();
//	return 1;
//}

int run()
{

	cout<<"Start Kalman Filter..."<<endl;

	KalmanFilter KF(4,0.1,0.001,0.001); 
	KF.A=*(cv::Mat_<float>(4,4)<<1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1); 
	KF.Z=*(cv::Mat_<float>(4,1)<<0,0,0,0);

	cout<<"Press Q to exit"<<endl;

	//prepare output
	cv::namedWindow("Tracking");
	cv::setMouseCallback("Tracking", on_mouse1, 0);
	cv::Mat backgrond = cv::Mat::zeros(400,400,CV_32FC3);
	cv::Mat Bkg;	


	

	for(;;)
	{
		backgrond.copyTo(Bkg);
		KF.Z=*(cv::Mat_<float>(4,1)<<mouse_info.x,mouse_info.y,KF.Z.at<float>(0)-mouse_info.x,KF.Z.at<float>(1)-mouse_info.y);
		KF.filter();

		

		circle(Bkg,cv::Point(mouse_info.x,mouse_info.y),2,cv::Scalar(255,0,0));
		circle(Bkg,cv::Point(KF.X.at<float>(0),KF.X.at<float>(1)),2,cv::Scalar(0,255,0));

		KF.drawTrack(Bkg,cv::Point(mouse_info.x,mouse_info.y),cv::Point(KF.X.at<float>(0),KF.X.at<float>(1)));
		imshow("Tracking",Bkg);


		char code = cv::waitKey(30);
		if( code == 27 || code == 'q' || code == 'Q' )
            break;
	}

	return 1;
}




